/*
 * Copyright 2021 <copyright holder> <email>
 * 
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * 
 *     http://www.apache.org/licenses/LICENSE-2.0
 * 
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "frontend.h"

#include <opencv2/opencv.hpp>

#include "config.h"
#include "feature.h"
#include "map.h"
#include "viewer.h"
#include "g2o_types.h"
#include "viewer.h"
#include "backend.h"

Frontend::Frontend::Frontend()
{
    gftt_=cv::GFTTDetector::create(Config::Get<int>("num_features"), 0.01, 20);
    num_features_init_=Config::Get<int>("num_features_init_");
    num_features_=Config::Get<int>("num_features");
}

bool Frontend::AddFrame(Frame::Ptr frame)
{
    current_frame_=frame;
    switch(status_){
        case FrontendStatus::INITING:
            StereoInit();
            break;
        case FrontendStatus::TRACKING_GOOD:
        case FrontendStatus::TRACKING_BAD:
            Track();
            break;
        case FrontendStatus::LOST:
            Reset();
            break;
    }
    last_frame_=current_frame_;
    return true;
}

bool Frontend::Track()
{
    if(last_frame_){
        current_frame_->SetPose(relative_motion_*last_frame_->Pose());
    }
    int num_track_last=TrackLastFrame();   //未使用
    tracking_inliers_=EstimateCurrentPose();
    
    if(tracking_inliers_>num_features_tracking_){
        status_=FrontendStatus::TRACKING_BAD;
    }
    else
    {
        status_=FrontendStatus::LOST;
    }
    
    InsertKeyframe();
    relative_motion_=current_frame_->Pose()*last_frame_->Pose().inverse();
    if(viewer_) viewer_->AddCurrentFrame(current_frame_);
    return true;
}

bool Frontend::InsertKeyframe()
{
    if(tracking_inliers_>=num_features_needed_for_keyframe_){
        return false;
    }  //新一帧的匹配特征大于新建帧的需求
    
    current_frame_->SetKeyFrame();
    map_->InsertKeyFrame(current_frame_);
    
    LOG(INFO)<<"Set frame"<<current_frame_->id_<<"as keyframe "<<current_frame_->keyframe_id_;
    
    SetObservationsForKeyFrame();
    DetectFeatures();  //detect new features;
    
    //track in right image
    FindFeaturewInRight();
    
    //triangulate map Points
    TriangulateNewPoints();
    
    //update backend because we have a new keyframe
    backend_->UpdateMap();
    
    if(viewer_) viewer_->UpdateMap();
    
    return true;
}

void Frontend::SetObservationsForKeyFrame()
{
    for(auto &feat : current_frame_->features_left_){
        auto mp=feat->map_point_.lock();
        if(mp) mp->AddObservation(feat);
}

int Frontend::TriangulateNewPoints(){
    std::vector<SE3> poses{camera_left_->pose(), camera_right_->pose()}
    SE3 current_pose_Twc =current_frame_->Pose().inverse();
    int cnt_triangulated_pts=0;
    for(size_t i=0; i<current_frame_->feature_left.size();++i){
        if(current_frame_->features_left_[i]->map_point_.expired() &&
            current_frame_->features_right_[i]!=nullptr){
            //左图的特征点为关联地图点且存在右图匹配点，尝试三角化
    















    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
}



